package frc.circuitrunners;

/*
 * StartApplication.java
 */
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.Watchdog;
/**
 * The main scheduler for the robot, work 
 */
public class Scheduler extends RobotBase {
    public static RobotSubsystem[] fibers = {
        new Drive(),
        //new BridgeDepressor(),
        new Shooter()//,
        //new Conveyor()
    };
    public static Watchdog watchdog;
    public static byte currentFiber = 0;
    public void startCompetition() {
        IO.init();
        watchdog = getWatchdog();
        for(int i = 0; i < fibers.length; i++){
            fibers[i].init();
        }
        while(true){ 
                feed();
                if(isEnabled()){
                    if(isAutonomous()){
                        //Stage One
                        long stageStartTime = System.currentTimeMillis();
                        while((System.currentTimeMillis() - stageStartTime) < 1000){
                            ((Shooter) fibers[2]).shoot(1.0);
                        }
                        //Stage Two
                        stageStartTime = System.currentTimeMillis();
                        while((System.currentTimeMillis() - stageStartTime) < 7000){
                            ((Conveyor) fibers[3]).uptake(true);
                        }
                        ((Shooter) fibers[2]).shoot(0.0);
                        ((Conveyor) fibers[3]).uptake(false);
                        //Stage Three
                        stageStartTime = System.currentTimeMillis();
                        while((System.currentTimeMillis() - stageStartTime) < 3000){
                            ((Drive) fibers[0]).driveCartesian(0, -1.0, 0);
                        }
                        ((Drive) fibers[0]).driveCartesian(0, 0, 0);
                        //Stage Four
                        stageStartTime = System.currentTimeMillis();
                        while((System.currentTimeMillis() - stageStartTime) < 3000){
                            ((BridgeDepressor) fibers[2]).depress(1.0);
                        }
                    //} else if(IO.getJoystickButton(IO.JOY_DRIVE, 3)){
                        //DashboardHandler.robotState = RobotSubsystem.STATE = RobotSubsystem.AUTOBALANCE;
                    } else if(IO.getJoystickButton(IO.JOY_SHOOTER,2)){
                        RobotSubsystem.STATE = RobotSubsystem.AUTOAIM;
                    } else {
                        RobotSubsystem.STATE = RobotSubsystem.TELEOP_NORMAL;
                    }
                } else {
                    RobotSubsystem.STATE = RobotSubsystem.DISABLED;
                }
                DashboardHandler.robotState = RobotSubsystem.STATE;
                try{
                    fibers[currentFiber].run();
                }catch(schedulerReturnException e){
                    //Ignore, perfectly normal
                }catch(Exception e){
                    e.printStackTrace();
                }catch(Error e){
                    e.printStackTrace();
                }
                //DashboardHandler.updateDashboard();
            if(++currentFiber >= fibers.length){
                currentFiber = 0;
            }
        }
    }
    /**
     * Returns to the main loop, moves onto the next fiber
     */
    public static void yield(){
        throw new schedulerReturnException();
    }
    /**
     * feeds the watchdog
     */
    public static void feed(){
        watchdog.feed();
    }
    
    static class schedulerReturnException extends RuntimeException{}
}